from xml.dom.minidom import Document
import zmq
import time
import numpy as np
# Local imports
import bluesky as bs
from bluesky import stack
from bluesky.tools import areafilter
from bluesky.core.walltime import Timer
import zmq
import time
import sys

#下面这部分是打印xml文件头部公共区域（苏雨）
doc = Document() #创建DOM文档对象（苏雨）
PropertyList = doc.createElement('PropertyList') #创建根元素（苏雨）
doc.appendChild(PropertyList)
scenario = doc.createElement('scenario')#book:Python处理XML之Minidom（苏雨）
PropertyList.appendChild(scenario)
name = doc.createElement('name')
name_text = doc.createTextNode('suyu_demo')
name.appendChild(name_text)
scenario.appendChild(name)

#在这里利用zmq来和BS建立连接，以便在FG中创建对应数量的AI飞机（苏雨）
context = zmq.Context()
#请注意，这里为了接收经纬度信息，打开了个socket（苏雨）
socket = context.socket(zmq.SUB)
socket.connect("tcp://localhost:5555")
#接收所有消息并用utf-8转码（苏雨）
socket.setsockopt(zmq.SUBSCRIBE,''.encode('utf-8'))
response = socket.recv().decode('utf-8')
#定义初始化开关，如果开关置于1，则定义数组并初始化，这是为了防止先创建飞机再启动节点，导致数组未定义的错误！（苏雨） 
global init
init = 1 
#定义经纬度开关，如果为0则开关处于中间，如果为1则数据进入lat，如果为2则数据进入lon（苏雨）
global latlon
latlon = 0
#定义上次的飞机总数，当飞机数量发送变化时候，才重新创建xml文件（苏雨）
global ac_number_last
ac_number_last = 0

#定义退出开关，建立好了xml就自动退出，防止错误！（苏雨）
global exit_flag
exit_flag = 0

while True:
    #如果第一次执行程序，则定义数组并初始化（苏雨）
    if init == 1:
        lat = []
        lon = []
        alt = []
        #初始化完成关闭初始化开关（苏雨）
        init = 0
    #response = socket.recv();
    response = socket.recv().decode('utf-8')
    #通过转义符区分每个采样周期所有飞机位置信息（苏雨）    
    if response == '"Position"':
        exit_flag = exit_flag + 1
        print (lat)
        print (lon)
        print (alt)
        if exit_flag > 5:#多跑几次保证数据稳定（苏雨）
            #在这里数组获得了所有飞机的参数（苏雨）
            ac_number = len(lat)
            print(ac_number)
            #监测数组元素个数是否发生变化，一旦变化就说明往里插入飞机了（苏雨）
            if ac_number != ac_number_last:
                print("number changed!") 
                ac_number_last = ac_number
                i = 0
                #循环往xml里面插入飞机的信息（苏雨）
                while i < ac_number:
                    ai_num = i + 1
                    alt_save = alt[i]
                    lat_save = lat[i]
                    lon_save = lon[i]            
                    #这里就可以循环创建新的AI飞机的xml参数文件了（苏雨）
                    entry = doc.createElement('entry')
                    scenario.appendChild(entry)
                    entry_callsign = doc.createElement('callsign')
                    entry_type = doc.createElement('type')
                    entry_class = doc.createElement('class')
                    entry_model = doc.createElement('model')
                    entry_altitude = doc.createElement('altitude')
                    entry_latitude = doc.createElement('latitude')
                    entry_longitude = doc.createElement('longitude')            
            
                    entry_callsign_text = doc.createTextNode('suyu_%d'%ai_num)
                    entry_type_text = doc.createTextNode('aircraft')
                    entry_class_text = doc.createTextNode('ufo')#这里飞机类型可以按需修改（苏雨）
                    entry_model_text = doc.createTextNode('Aircraft/c182/Models/c182-ai.xml')#这里飞机文件可以按需修改（苏雨）
                    entry_altitude_text = doc.createTextNode('%f'%alt_save)
                    entry_latitude_text = doc.createTextNode('%f'%lat_save)
                    entry_longitude_text = doc.createTextNode('%f'%lon_save)                            

                    entry.appendChild(entry_callsign)
                    entry.appendChild(entry_type)
                    entry.appendChild(entry_class)
                    entry.appendChild(entry_model)
                    entry.appendChild(entry_altitude)
                    entry.appendChild(entry_latitude)
                    entry.appendChild(entry_longitude)  
                          
                    entry_callsign.appendChild(entry_callsign_text)
                    entry_type.appendChild(entry_type_text)
                    entry_class.appendChild(entry_class_text)
                    entry_model.appendChild(entry_model_text)
                    entry_altitude.appendChild(entry_altitude_text)
                    entry_latitude.appendChild(entry_latitude_text)
                    entry_longitude.appendChild(entry_longitude_text)   
                    i = i + 1         
            #生成xml文件，注意要写到循环外面！！（苏雨）
            scenario.appendChild(entry)            
            #将DOM对象doc写入文件，可以指定特定路径和文件名（苏雨）
            f = open('/home/suyu/suyu/FlightGear_connector/protocol/TEST.xml','w')#可以在这里修改xml文件的存储路径（苏雨）
            f.write(doc.toprettyxml(indent = ''))
            f.close()
            break
                                 
        #初始化数组，用来存储采样周期内所有飞机位置信息（苏雨）
        lat = []
        lon = []
        alt = []
        latlon = 0    
    else:
        if response == '"Lat"':
            latlon = 1
        elif response == '"Lon"':    
            latlon = 2
        elif response == '"Alt"':    
            latlon = 3             
        else:                     
            if latlon == 1:
                #将字符串类型转换为浮点数（苏雨）
                transfer = float(response)   
                #向数组末尾插信息（苏雨）
                lat.append(transfer)
                latlon = 0                     
            if latlon == 2:
                #将字符串类型转换为浮点数（苏雨）
                transfer = float(response)   
                #向数组末尾插信息（苏雨）
                lon.append(transfer)
                latlon = 0
            if latlon == 3:
                #将字符串类型转换为浮点数（苏雨）
                transfer = float(response)   
                #向数组末尾插信息（苏雨）
                alt.append(transfer)
                latlon = 0


















